/*
 * =====================================================================================
 *
 *       Filename:  blas.h
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  2009年06月03日 19时00分04秒
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Ying Wang (WY), ywang\nlpr.ia.ac.cn
 *        Company:  Institute of Automation, Chinese Academy of Sciences
 *
 * =====================================================================================
 */

#ifndef BLAS_H
#define BLAS_H

#include "ncvector.h"
#include "ncmatrix.h"
#include <cmath>
#include <algorithm>

/**
 * AX = B  ===>  [A|B] ---> [I|inv(A)B]
 */
template<typename T>
NCmatrix<T> gaussjordan (const NCmatrix<T>& matA, const NCmatrix<T>& matB)
{
	int i,j, i_row,j_column,A_row=matA.row(),A_column=matA.column(),B_column=matB.column();
	NCmatrix<T> A(matA), B(matB);
	//NCvector<int> ipivot(A_row,0);
	double tiny=1e-10;
	double big,temp;
	double factor;
	int swap_row;

	//eliminate by row operation upper triangle
	for ( i=0; i<A_row; i++ )
	{
		big = 0;
		for( j=i; j<A_row; j++ )
		{
			if ( (temp=fabs(A[j][i])) > big )
			{
				big = temp;
				swap_row = j;
			}
		}
		if ( swap_row != i )
		{
			//swap swap_row and ith row
			for ( j_column=i; j_column<A_column; j_column++ )
			{
				std::swap(A[i][j_column],A[swap_row][j_column]);
			}
			for ( j_column=0; j_column<B_column; j_column++ )
			{
				std::swap(B[i][j_column],B[swap_row][j_column]);
			}
		}
		//emilitate
		for( i_row=i+1; i_row<A_row; i_row++ )
		{
			if(fabs(A[i_row][i])>tiny)
			{
				factor = A[i_row][i]/A[i][i];
				A[i_row][i] = 0;
				for( j_column = i+1; j_column<A_column; j_column++ )
				{
					A[i_row][j_column] -= factor*A[i][j_column];
				}
				for( j_column = 0; j_column<B_column; j_column++ )
				{
					B[i_row][j_column] -= factor*B[i][j_column];
				}
			}
		}

	}

	//diag
	for (i=A_row-1;i>=0;i--)
	{
		if(fabs(A[i][i])>tiny)
		{
			for(j=i-1;j>=0;j--)
			{
				if(fabs(A[j][i])>tiny)
				{
					factor = A[j][i]/A[i][i];
					for(j_column=0;j_column<B_column;j_column++)
					{
						B[j][j_column] -= factor*B[i][j_column];
					}
					A[j][i] = 0;
				}

			}
		}
	}
	//
	for ( i=0; i<A_row; i++ )
	{
		if (fabs(A[i][i]) > tiny )
		{
			for(j=0;j<B_column;j++)
			{
				B[i][j] /= A[i][i];
			}
			A[i][i] = 1;
		}
	}
	return B;
}

/**
 * PA = LU ===> U is upper triangle matrix,
 * A = P*lu
 */
template<typename T>
NCvector<int> LUdcmp(const NCmatrix<T> &mat, NCmatrix<T> &lu)
{
//	const double tiny = 1.0e-10;
	int i, imax, j,k,n=mat.row();
	lu = mat;
	double big,temp;
	NCvector<T> vv(n);
	NCvector<int> indx(n);
	double d=1.0;
	for (i=0; i<n; i++)
	{
		big = 0.0;
		for( j=0; j<n; j++)
			if ( (temp=fabs(lu[i][j])) > big ) big = temp;
		if ( big ==0 ) throw ("Singular matrix in lu decmp" );
		vv[i] = 1.0/big;
	}
	for(k=0;k<n;k++)
	{
		big = 0.0;
		for(i=k;i<n;i++)
		{
			temp = vv[i] * fabs(lu[i][k]);
			if(temp > big)
			{
				big = temp;
				imax = i;
			}
		}
		if( k != imax )
		{
			for( j=0; j<n; j++ )
			{
				temp = lu[imax][j];
				lu[imax][j] = lu[k][j];
				lu[k][j] = temp;
			}
			d = -d;
			vv[imax] = vv[k];
		}
		indx[k] = imax;
		for(i=k+1;i<n;i++)
		{
			temp = lu[i][k] /= lu[k][k];
			for(j=k+1;j<n;j++)
				lu[i][j] -= temp*lu[k][j];
		}
	}
	return indx;
}

/**
 * \brief Solve the linear equation  Ax = b using Gauss Elimination with Backsubstitution
 *
 * \param matA parameter A
 * \param vecb parameter b
 *
 * \return return the solution of the equation.
 */
template<typename T>
inline NCvector<T> solve (const NCmatrix<T> &matA, const NCvector<T> &vecb)
{
	int i, j, i_row, j_column, A_row = matA.row(), A_column = matA.column();
	NCmatrix<T> A(matA);
	NCvector<T> b(vecb);
	NCvector<T> x(A_column);
	const double tiny = 1e-10;
	double big,temp;
	double factor;
	int swap_row;
	for( i=0; i<A_row; i++ )
	{
		big = 0;
		for( j=i; j<A_row; j++ )
		{
			if ( (temp=fabs(A[j][i])) > big )
			{
				big = temp;
				 swap_row = j;
			}
		}
		if ( swap_row != i )
		{
			//swap A's i-th row and swap_row-th row;
			for ( j_column = i; j_column<A_column; j_column++ )
			{
				temp = A[i][j_column];
				A[i][j_column] = A[swap_row][j_column];
				A[swap_row][j_column] = temp;
			}
			// swap b[i] and b[swap_row]
			temp = b[i];
			b[i] = b[swap_row];
			b[swap_row] = temp;
		}

		// upper triangle
		for( i_row = i+1; i_row<A_row; i_row++ )
		{
			if(fabs(A[i_row][i])>tiny)
			{
				factor = A[i_row][i]/A[i][i];
				A[i_row][i] = 0;
				for( j_column = i+1; j_column<A_column; j_column++ )
				{
					A[i_row][j_column] -= factor*A[i][j_column];
				}
				b[i_row] -= factor * b[i];
			}
		}
	}

	//Backsubstitution
	for( i=A_column-1; i>=0; i-- )
	{
		temp = 0;
		for( j=i+1; j<A_column; j++ )
		{
			temp += A[i][j] * x[j];
		}
		x[i] = (b[i]-temp)/A[i][i];
	}
	return x;
}

/**
 * \brief Solve the equation A*X = B with using Gauss Elimination with Backsubstitution
 *
 * \param matA parameter A
 * \param matB parameter B
 *
 * \return return the solution of the equation.
 *
 */
template<typename T>
inline NCmatrix<T> solve (const NCmatrix<T> &matA, const NCmatrix<T> &matB)
{
	int i, j, i_row, j_column, A_row = matA.row(), A_column = matA.column(), B_column = matB.column();
	NCmatrix<T> A(matA);
	NCmatrix<T> B(matB);
	NCmatrix<T> X(A_column,B_column);
	const double tiny = 1e-10;
	double big,temp;
	double factor;
	int swap_row;
	for ( i=0; i<A_row; i++ )
	{
		big = 0;
		for ( j=i; j<A_row; j++ )
		{
			if( (temp=fabs(A[j][i])) > big )
			{
				big = temp;
				swap_row = j;
			}
		}
		if ( swap_row != i )
		{
			for( j_column=i; j_column<A_column; j_column++ )
			{
				temp = A[i][j_column];
				A[i][j_column] = A[swap_row][j_column];
				A[swap_row][j_column] = temp;
			}
			for( j_column=0; j_column<B_column; j_column++ )
			{
				temp = B[i][j_column];
				B[i][j_column] = B[swap_row][j_column];
				B[swap_row][j_column] = temp;
			}
		}
		for ( i_row = i+1; i_row<A_row; i_row++ )
		{
			if( fabs(A[i_row][i]) > tiny )
			{
				factor = A[i_row][i]/A[i][i];
				A[i_row][i] = 0;
				for( j_column = i+1; j_column<A_column; j_column++ )
				{
					A[i_row][j_column] -= factor*A[i][j_column];
				}
				for( j_column = 0; j_column<B_column; j_column++ )
				{
					B[i_row][j_column] -= factor * B[i][j_column];
				}
			}
		}
	}
	for( j_column=0; j_column<B_column; j_column++ )
	{
		for( i=A_column-1; i>=0; i-- )
		{
			temp = 0;
			for( j=i+1; j<A_column; j++ )
			{
				temp += A[i][j] * X[j][j_column];
			}
			X[i][j_column] = (B[i][j_column]-temp)/A[i][i];
		}
	}
	return X;

}
/**
 * \brief Cholesky Decomposition for positive symmetric matrix
 *
 * \param mat Input positive symmetric matrix
 *
 * \return return the lower triangle matrix L
 *
 */
template<typename T>
NCmatrix<T> choleskydcmp (const NCmatrix<T> &mat)
{
	int i,k,j, row = mat.row();
	double sum;
	NCmatrix<T> L(mat);
	for ( i=0;i<row;i++ )
	{
		for (j=i;j<row;j++)
		{
			for (sum = L[i][j], k=i-1;k>=0;k--)	sum -= L[i][k] * L[j][k];
			if (i == j)
			{
				if (sum<=0.0)
				{
					std::cerr<< " cholesky failed!" << std::endl;
					throw ( "cholesky failed!" );
				}
				L[i][i] = sqrt(sum);
			}
			else
				L[j][i] = sum/L[i][i];
		}

	}
	for (i=0;i<row;i++)
		for (j=0;j<i;j++)
			L[j][i] = 0;
	return L;
}

#endif
